{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "  # We must install required packages if we are in Google Colab\n",
    "  import google.colab\n",
    "  %pip install roboticstoolbox-python>=1.0.2\n",
    "  %pip install qpsolvers[quadprog]\n",
    "  COLAB = True\n",
    "except:\n",
    "    pass"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Numerical Inverse Kinematics Benchmark\n",
    "\n",
    "<br>\n",
    "\n",
    "In this Notebook we benchmark the performance of each numerical IK solver we provide with the toolbox.\n",
    "\n",
    "There are several ways to use IK solvers with the Toolbox:\n",
    "\n",
    "* The **fast** methods which are implemented in C++ and wrapped in Python. These methods are available from ``robot`` and ``ets`` classes and start with ``ik_``. For example ``ik_LM`` is a fast C++ based solver.\n",
    "\n",
    "* The **slow** methods which are implemented in Python. These methods are available from ``robot`` and ``ets`` classes and start with ``ikine_``. For example ``ikine_LM`` is a slow Python based solver.\n",
    "\n",
    "* The **slow** class-based operation which is implemented in Python. These classes inherit from the ``IKSolver`` class and start with ``IK_``. For example ``IK_LM`` is a class which provides a slow Python based solver.\n",
    "\n",
    "### Contents\n",
    "\n",
    "[1.0 Fast IK Methods](#1)\n",
    "\n",
    "[2.0 Python IK Methods](#2)\n",
    "\n",
    "[3.0 Python Class Based IK Methods](#3)\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We will do the imports required for this notebook here\n",
    "\n",
    "# numpy provides import array and linear algebra utilities\n",
    "import numpy as np\n",
    "\n",
    "# the robotics toolbox provides robotics specific functionality\n",
    "import roboticstoolbox as rtb\n",
    "\n",
    "# ansitable is a great package for printing tables in a terminal\n",
    "from ansitable import ANSITable\n",
    "\n",
    "# a package for creating dynamic progress bars\n",
    "from progress.bar import Bar\n",
    "\n",
    "# swift is a lightweight browser-based simulator which comes with the toolbox\n",
    "from swift import Swift\n",
    "\n",
    "# spatialgeometry is a utility package for dealing with geometric objects\n",
    "import spatialgeometry as sg\n",
    "\n",
    "# provides sleep functionaly\n",
    "import time\n",
    "\n",
    "# suppress warnings\n",
    "import warnings\n",
    "warnings.filterwarnings('ignore')"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='1'></a>\n",
    "\n",
    "### 1.0 Fast IK Methods\n",
    "---"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Our robot and ETS\n",
    "robot = rtb.models.Panda()\n",
    "ets = robot.ets()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Number of problems to solve\n",
    "speed_problems = 10000\n",
    "\n",
    "# Cartesion DoF priority matrix\n",
    "mask = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])\n",
    "\n",
    "# random valid q values which will define Tep\n",
    "q_rand = ets.random_q(speed_problems)\n",
    "\n",
    "# Our desired end-effector poses\n",
    "Tep = np.zeros((speed_problems, 4, 4))\n",
    "\n",
    "for i in range(speed_problems):\n",
    "    Tep[i] = ets.eval(q_rand[i])\n",
    "\n",
    "# Maximum iterations allowed in a search\n",
    "ilimit = 30\n",
    "\n",
    "# Maximum searches allowed per problem\n",
    "slimit = 100\n",
    "\n",
    "# Solution tolerance\n",
    "tol = 1e-6\n",
    "\n",
    "joint_limits = False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "speed_solvers = [\n",
    "    lambda Tep: ets.ik_NR(\n",
    "        Tep,\n",
    "        ilimit=ilimit,\n",
    "        slimit=slimit,\n",
    "        tol=tol,\n",
    "        mask=mask,\n",
    "        pinv=True,\n",
    "        joint_limits=joint_limits,\n",
    "    ),\n",
    "    lambda Tep: ets.ik_GN(\n",
    "        Tep,\n",
    "        ilimit=ilimit,\n",
    "        slimit=slimit,\n",
    "        tol=tol,\n",
    "        mask=mask,\n",
    "        pinv=True,\n",
    "        joint_limits=joint_limits,\n",
    "    ),\n",
    "    lambda Tep: ets.ik_LM(\n",
    "        Tep, ilimit=ilimit, slimit=slimit, tol=tol, mask=mask, k=1e-4, joint_limits=joint_limits, method=\"wampler\"\n",
    "    ),\n",
    "    lambda Tep: ets.ik_LM(\n",
    "        Tep,\n",
    "        q0=None,\n",
    "        ilimit=ilimit,\n",
    "        slimit=slimit,\n",
    "        tol=tol,\n",
    "        mask=mask,\n",
    "        k=0.1,\n",
    "        joint_limits=joint_limits,\n",
    "        method=\"chan\",\n",
    "    ),\n",
    "    lambda Tep: ets.ik_LM(\n",
    "        Tep, ilimit=ilimit, slimit=slimit, tol=tol, mask=mask, k=0.0001, joint_limits=joint_limits, method=\"sugihara\"\n",
    "    ),\n",
    "]\n",
    "\n",
    "speed_names = [\n",
    "    \"Newton Raphson (pinv=True)\",\n",
    "    \"Gauss Newton (pinv=True)\",\n",
    "    \"LM Wampler 1e-4\",\n",
    "    \"LM Chan 0.1\",\n",
    "    \"LM Sugihara 0.0001\",\n",
    "]\n",
    "\n",
    "times = []"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Solving with Newton Raphson (pinv=True)\n",
      "Solving with Gauss Newton (pinv=True)\n",
      "Solving with LM Wampler 1e-4\n",
      "Solving with LM Chan 0.1\n",
      "Solving with LM Sugihara 0.0001\n",
      "\n",
      "Numerical Inverse Kinematics Methods Times Compared over 10000 problems\n",
      "\n",
      "┌───────────────────────────┬────────────────┬────────────────────────────────┐\n",
      "│                    Method │ Total Time (s) │ Average Time per Solution (μs) │\n",
      "├───────────────────────────┼────────────────┼────────────────────────────────┤\n",
      "│Newton Raphson (pinv=True)\u001b[0m │         1.8708\u001b[0m │                       187.0795\u001b[0m │\n",
      "│  Gauss Newton (pinv=True)\u001b[0m │          2.474\u001b[0m │                       247.3969\u001b[0m │\n",
      "│           LM Wampler 1e-4\u001b[0m │         0.6079\u001b[0m │                        60.7928\u001b[0m │\n",
      "│               LM Chan 0.1\u001b[0m │         0.2724\u001b[0m │                        27.2353\u001b[0m │\n",
      "│        LM Sugihara 0.0001\u001b[0m │         0.3546\u001b[0m │                        35.4575\u001b[0m │\n",
      "└───────────────────────────┴────────────────┴────────────────────────────────┘\n",
      "\n"
     ]
    }
   ],
   "source": [
    "for name, solver in zip(speed_names, speed_solvers):\n",
    "    print(f\"Solving with {name}\")\n",
    "\n",
    "    start = time.time()\n",
    "\n",
    "    for i in range(speed_problems):\n",
    "        solver(Tep[i])\n",
    "\n",
    "    total_time = time.time() - start\n",
    "    times.append(total_time)\n",
    "\n",
    "\n",
    "print(f\"\\nNumerical Inverse Kinematics Methods Times Compared over {speed_problems} problems\\n\")\n",
    "\n",
    "table = ANSITable(\n",
    "    \"Method\",\n",
    "    \"Total Time (s)\",\n",
    "    \"Average Time per Solution (μs)\",\n",
    "    border=\"thin\",\n",
    ")\n",
    "\n",
    "for name, t in zip(speed_names, times):\n",
    "    table.row(\n",
    "        name,\n",
    "        np.round(t, 4),\n",
    "        np.round((t / speed_problems) * 1e6, 4),\n",
    "    )\n",
    "\n",
    "table.print()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "rtb",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.0"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "4e8f2602a343ae62ff483b4df7805ab0eb447ccc1a56caf0631d5620d36b21ab"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
